within ThreeD_MBS_Dynamics.Examples.Snakeboard;

model TestSnakeboardSectioned
  // With Rotor and Control Phi & Psi
  parameter SI.Acceleration g = 9.81;
  parameter SI.Acceleration[3] Gravity = {0,-g,0};
  parameter SI.Velocity delta = 10^(-4);
  parameter SI.Mass m_bar = 1;
  parameter SI.Mass m_rot = 1;
  parameter SI.Mass m = LeftWheels.m + RightWheels.m + m_bar + m_rot;
  // Radius of Disc
  parameter SI.Radius r1 = 0.4;
  // Radius of Rod
  parameter SI.Radius r2 = 0.15;
  // Horizontal Length of Rod
  // Radius of Rotor
  parameter SI.Radius r3 = 0.5;
  parameter SI.Length L = 1.5;
  // Length of Bar
  parameter SI.Length LD = 5;
  // Width of Bar
  parameter SI.Length WD = 0.5;
  // Height of Bar
  parameter SI.Length HD = 0.2;
  // Height of Rotor
  parameter SI.Length HR = 0.4;
  parameter SI.Length h = 0.2 + r2 + HD/2;
  parameter SI.Length hr = r1 + h + HD/2;
  parameter SI.AngularVelocity omega0 = 0;
  parameter SI.AngularVelocity omega1 = 0.3;
  parameter SI.AngularVelocity omega_r = 1;
  //  parameter Real phi2=-Modelica.Constants.pi/8;
  parameter SI.Position[3] r0 = {-0.5*LD + r2,r1,0};
  parameter SI.Velocity[3] v0 = {0,0,0};
  parameter SI.Angle a_psi = 0.7;
  parameter SI.Angle b_psi = 0;
  parameter SI.Angle a_phi = 0.3;
  parameter SI.Angle b_phi = 0.01;
  parameter SI.Angle phi1 = a_phi*sin(b_phi);
  //  parameter Real phi1=0;
  //  parameter SI.AngularVelocity w_phi=1;
  parameter Stiffness c = 1000;
  parameter Viscosity d = 5000;
  RollingWheelSet LeftWheels(
    Gravity = Gravity,
    c = c,
    d = d,
    r1 = r1,
    r2 = r2,
    L=L,
    r0=r0,
    v0=v0 + cross({0, omega0, 0}, {-0.5*LD + r2, 0, 0}),
    omega1 = omega0 - omega1,
    phi = -phi1) annotation (extent=[-70,-94; -30,-54], rotation=90);
  RollingWheelSet RightWheels(
    Gravity=Gravity,
    c=c,
    d=d,
    r1=r1,
    r2=r2,
    L=L,
    r0=r0 + {LD - 2*r2,0,0},
    v0=v0 + cross({0,omega0,0}, {0.5*LD - r2,0,0}),
    omega1=omega0 + omega1,
    phi=phi1) annotation (extent=[70,-94; 30,-54], rotation=90);
  LeftBar LBar(
    Gravity=Gravity,
    LD=LD,
    WD=WD,
    HD=HD,
    q(start={1,0,0,0}),
    omega(start={0,omega0,0}),
    m=0.5*m_bar,
    I=0.5*m_bar/12*[0.25*LD^2 + HD^2, 0, 0; 0, WD^2 + HD^2, 0; 0, 0, 0.25*LD^
        2 + WD^2],
    r(start={-0.25*LD,r1 + h,0}),
    v(start=v0 + cross({0,omega0,0}, {-LD/4,0,0}))) 
    annotation (extent=[-70,0; -30,40]);
  Base FLoor annotation (extent=[-20,-74; 20,-34]);
  FixedServoJoint LeftJoint(
    nA={0,1,0},
    nB={0,1,0},
    rA={0,h,0},
    rB={-0.25*LD + r2,0,0}) annotation (extent=[-70,-48; -30,-8],  rotation=90);
  FixedServoJoint RightJoint(
    nA={0,1,0},
    nB={0,1,0},
    rA={0,h,0},
    rB={0.25*LD - r2,0,0}) annotation (extent=[70,-48; 30,-8],  rotation=90);
  FixedServoJoint CJoint(
    nA={0,1,0},
    nB={0,1,0},
    rB={0,0,0},
    rA={LD/4,HD/2,0}) annotation (extent=[-20,50; 20,90]);
  Rotor RoBody(
    Gravity=Gravity,
    m=m_rot,
    r3=r3,
    HR=HR,
    q(start={1,0,0,0}),
    I=m_rot*[0.25*r3^2, 0, 0; 0, 0.5*r3^2, 0; 0, 0, 0.25*r3^2],
    v(start=v0),
    omega(start={0,omega_r*a_psi,0}),
    r(start={0,hr,0})) annotation (extent=[30,50; 70,90]);
  SpringJoint Spring(
    c=c,
    d=d,
    nA={1,0,0},
    nB={1,0,0},
    rA={LD/4,0,0},
    rB={-LD/4,0,0}) annotation (extent=[-20,0; 20,40]);
  RightBar RBar(
    Gravity=Gravity,
    q(start={1,0,0,0}),
    m=0.5*m_bar,
    I=0.5*m_bar/12*[0.25*LD^2 + HD^2, 0, 0; 0, WD^2 + HD^2, 0; 0, 0, 0.25*LD^
        2 + WD^2],
    r(start={0.25*LD,r1 + h,0}),
    v(start=v0 + cross({0,omega0,0}, {LD/4,0,0})),
    omega(start={0,omega0,0})) annotation (extent=[30,0; 70,40]);
  annotation (
    Diagram,
    experiment(
      StopTime=50,
      NumberOfIntervals=15000,
      Tolerance=1e-012),
    experimentSetupOutput,
    DymolaStoredErrors);
  Test_Kuleshov IdealModel(
    J=LBar.I[2, 2],
    Jr=RoBody.I[2, 2],
    L=LD/2,
    m=m_bar + m_rot + 2*(2*LeftWheels.m_disc + LeftWheels.m_rod),
    Jp=LeftWheels.Rod1.I[2, 2] + 2*(LeftWheels.Disc1.I[2, 2] + LeftWheels.m_disc*L
        ^2/4),
    a_psi=a_psi,
    b_psi=b_psi,
    w0_psi=omega_r,
    a_phi=a_phi,
    b_phi=b_phi,
    w0_phi=omega1/a_phi/cos(b_phi)) annotation (extent=[-96,50; -56,90]);
  SI.Angle theta;
  SI.Angle psi;
  SI.Angle phi_pr;
  SI.AngularAcceleration eps_psi;
  SI.AngularVelocity om_psi;
  SI.AngularAcceleration eps_phi;
  SI.AngularVelocity om_phi;
  SI.Position delta_x;
  SI.Position delta_z;
  SI.Angle delta_psi;
  SI.Angle delta_phi;
  SI.Torque delta_mom;
  SI.Energy Ed;
  SI.Energy Kt;
  SI.Energy Kr;
equation
  Ed = LeftWheels.Ed + RightWheels.Ed + Spring.Ed;
  //  K = 0.5*IdealModel.Jp*LeftWheels.Rod1.omega*LeftWheels.Rod1.omega +
  //      0.5*IdealModel.Jp*RightWheels.Rod1.omega*RightWheels.Rod1.omega;
  //  Kt = 0.5*(LeftWheels.Disc1.m + LeftWheels.Disc2.m + LeftWheels.Rod1.m +
  //      RightWheels.Disc1.m + RightWheels.Disc2.m + RightWheels.Rod1.m +
  //      LBar.m + RBar.m + RoBody.m)*RoBody.v*RoBody.v;
  Kt = LBar.K + RBar.K + RoBody.K + 0.5*IdealModel.Jp*LeftWheels.Rod1.
    omega*LeftWheels.Rod1.omega + 0.5*IdealModel.Jp*RightWheels.Rod1.omega*
    RightWheels.Rod1.omega;
  Kr = 0.5*LeftWheels.Disc1.I[3, 3]*LeftWheels.Disc1.omega[3]*LeftWheels.Disc1.
    omega[3] + 0.5*LeftWheels.Disc2.I[3, 3]*LeftWheels.Disc2.omega[3]*LeftWheels.
     Disc2.omega[3] + 0.5*RightWheels.Disc1.I[3, 3]*RightWheels.Disc1.omega[3]*
    RightWheels.Disc1.omega[3] + 0.5*RightWheels.Disc2.I[3, 3]*RightWheels.Disc2.
    omega[3]*RightWheels.Disc2.omega[3] + 0.5*(LeftWheels.Rod1.m + LeftWheels.
    Disc1.m + LeftWheels.Disc2.m)*LeftWheels.Rod1.v*LeftWheels.Rod1.v + 0.5*(
    RightWheels.Rod1.m + RightWheels.Disc1.m + RightWheels.Disc2.m)*RightWheels.Rod1.
    v*RightWheels.Rod1.v;
  //  K = LeftWheels.K + RightWheels.K + LBar.K + RBar.K + RoBody.K;
  delta_x = IdealModel.x - Spring.RB[1];
  delta_z = IdealModel.z - Spring.RB[3];
  delta_psi = IdealModel.psi - psi;
  delta_phi = IdealModel.phi - phi_pr;
  delta_mom = IdealModel.u_psi - RoBody.M[2];
  // Control for WheelSet (phi)
  LeftJoint.Control = -RightJoint.Control;
  RightJoint.Control = omega1^2/a_phi/(cos(b_phi))^2*sin(omega1/a_phi/cos(b_phi)*time + b_phi);

  // Control for Bar (psi)
  CJoint.Control = -omega_r^2*a_psi*sin(omega_r*time + b_psi);

  theta = Modelica.Math.asin((LBar.T*{1,0,0})*{0,0,-1});
  psi = Modelica.Math.asin((RoBody.T*{1,0,0})*{0,0,-1});
  phi_pr = Modelica.Math.asin((RightWheels.Rod1.T*{0,0,1})*(RBar.T*{1,0,0}));
  eps_psi = (RoBody.OutPort.epsilon - LBar.OutPort.epsilon)*RightJoint.nAi;
  om_psi = (RoBody.OutPort.omega - LBar.OutPort.omega)*RightJoint.nAi;
  eps_phi = (RightWheels.Rod1.OutPort.epsilon - RBar.OutPort.epsilon)*RightJoint.
    nAi;
  om_phi = (RightWheels.Rod1.OutPort.omega - RBar.OutPort.omega)*RightJoint.nAi;
  connect(LeftWheels.OutPortBase, FLoor.OutPort) 
    annotation (points=[-32,-80; 0,-80; 0,-72]);
  connect(RightWheels.OutPortBase, FLoor.OutPort) 
    annotation (points=[32,-80; 0,-80; 0,-72]);
  connect(LBar.InPortJoint,CJoint. OutPortA) 
    annotation (points=[-50,38; -50,94; -6,94; -6,88]);
  connect(CJoint.InPortB,RoBody. OutPort) 
    annotation (points=[6,52; 6,46; 50,46; 50,52]);
  connect(Spring.OutPortB,RBar. InPortJointA) annotation (points=[6,38; 6,44;
        42,44; 42,38], style(color=3, rgbcolor={0,0,255}));
  connect(LBar.InPortJointB,Spring. OutPortA) annotation (points=[-40,38; -40,
        44; -6,44; -6,38],   style(color=3, rgbcolor={0,0,255}));
  connect(Spring.InPortA,LBar. OutPort) annotation (points=[-6,2; -6,-4; -50,-4;
        -50,2], style(color=3, rgbcolor={0,0,255}));
  connect(Spring.InPortB,RBar. OutPort) annotation (points=[6,2; 6,-4; 50,-4;
        50,2], style(color=3, rgbcolor={0,0,255}));
  connect(CJoint.OutPortB, RoBody.InPort) annotation (points=[6,88; 6,94; 50,94;
        50,88], style(color=3, rgbcolor={0,0,255}));
  connect(RightJoint.OutPortA, RightWheels.InPortJoint) annotation (points=[68,
        -34; 76,-34; 76,-74; 68,-74], style(color=3, rgbcolor={0,0,255}));
  connect(RightJoint.InPortA, RightWheels.OutPortJoint) annotation (points=[32,
        -34; 24,-34; 24,-68; 32,-68], style(color=3, rgbcolor={0,0,255}));
  connect(LeftJoint.InPortA, LeftWheels.OutPortJoint) annotation (points=[-32,
        -34; -24,-34; -24,-68; -32,-68], style(color=3, rgbcolor={0,0,255}));
  connect(LeftJoint.OutPortA, LeftWheels.InPortJoint) annotation (points=[-68,
        -34; -76,-34; -76,-74; -68,-74], style(color=3, rgbcolor={0,0,255}));
  connect(LBar.OutPort, LeftJoint.InPortB) annotation (points=[-50,2; -50,-4;
        -24,-4; -24,-22; -32,-22], style(color=3, rgbcolor={0,0,255}));
  connect(CJoint.InPortA, LBar.OutPort) annotation (points=[-6,52; -6,48; -24,
        48; -24,-4; -50,-4; -50,2], style(color=3, rgbcolor={0,0,255}));
  connect(RBar.OutPort, RightJoint.InPortB) annotation (points=[50,2; 50,-4; 24,
        -4; 24,-22; 32,-22], style(color=3, rgbcolor={0,0,255}));
  connect(RBar.InPortJointB, RightJoint.OutPortB) annotation (points=[58,38; 58,
        44; 76,44; 76,-22; 68,-22], style(color=3, rgbcolor={0,0,255}));
  connect(LBar.InPortJointA, LeftJoint.OutPortB) annotation (points=[-60,38;
        -60,44; -76,44; -76,-22; -68,-22], style(color=3, rgbcolor={0,0,255}));
end TestSnakeboardSectioned;
